﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace InverseKinematics
{
    /// <summary>
    /// The BoneChain class repressents a list of bones which are always connected once.s
    /// On the one hand you can add new bones and every bone's source is the last bone's destination
    /// on the other hand you can use the cyclic coordinate descent to change the bones' positions. 
    /// </summary>
    public class BoneChain
    {
        /// <summary>
        /// The last bone that were created
        /// </summary>
        private Bone lastBone;

        /// <summary>
        /// All the concatenated bones 
        /// </summary>
        private List<Bone> bones;

        /// <summary>
        /// Creates an empty bone chain
        /// Added Bones will be affected by inverse kinematics
        /// </summary>
        public BoneChain()
        {
            this.bones = new List<Bone>();
        }

        /// <summary>
        /// Draws all the bones in this chain
        /// </summary>
        /// <param name="device"></param>
        public void Draw(GraphicsDevice device)
        {
            foreach (Bone bone in bones) bone.Draw(device);
        }

        /// <summary>
        /// Creates a bone
        /// Every bone's destination is the next bone's source 
        /// </summary>
        /// <param name="v">the bone's destination</param>
        /// <param name="click">if true it sets the bone with its coordinate and adds the next bone</param>
        public void CreateBone(Vector3 v, bool click)
        {
            if (click)
            {
                //if it is the first bone it will create the bone's source at the destination point
                //so it need not to start at the coordinates(0/0/0)
                if (bones.Count == 0)
                {
                    lastBone = new Bone(v, Vector3.Zero);
                    bones.Add(lastBone);
                }
                else
                {
                    Bone temp = new Bone(lastBone, v);
                    bones.Add(temp);
                    lastBone = temp;
                }
            }
            if (lastBone != null)
            {
                lastBone.SetLocalDestinationbyAWorldDestination(v);
            }

        }
        /// <summary>
        /// The Cyclic Coordinate Descent
        /// </summary>
        /// <param name="destination">Where the bones should be adjusted</param>
        /// <param name="gameTime"></param>
        public void CalculateCCD(Vector3 destination, GameTime gameTime)
        {

                // iterating the bones reverse
                int index = bones.Count - 1;
                while (index >= 0)
                {
                    //getting the vector between the new destination and the joint's world position
                    Vector3 jointWorldPositionToDestination = destination - bones.ElementAt(index).WorldCoordinate;

                    //getting the vector between the end effector and the joint's world position
                    Vector3 boneWorldToEndEffector = bones.Last().Effector - bones.ElementAt(index).WorldCoordinate;
                    
                    //calculate the rotation axis which is the cross product of the destination
                    Vector3 cross = Vector3.Cross(jointWorldPositionToDestination, boneWorldToEndEffector);

                    //normalizing that rotation axis
                    cross.Normalize();
                    //check if there occured divisions by 0 
                    if (float.IsNaN(cross.X) || float.IsNaN(cross.Y) || float.IsNaN(cross.Z))
                    //take a temporary vector
                    cross = Vector3.UnitZ;

                    // calculate the angle between jointWorldPositionToDestination and boneWorldToEndEffector
                    // in regard of the rotation axis
                    float angle = CalculateAngle(jointWorldPositionToDestination, boneWorldToEndEffector, cross);
                    if (float.IsNaN(angle)) angle = 0;

                    //create a matrix for the roation of this bone's destination
                    Matrix m = Matrix.CreateFromAxisAngle(cross, angle);

                    // rotate the destination
                    bones.ElementAt(index).Destination = Vector3.Transform(bones.ElementAt(index).Destination, m);
                    
                    // update all bones which are affected by this bone
                    UpdateBones(index);
                    index--;
                }
        }

        /// <summary>
        /// While CalculateCCD changes the destinations of all the bones, 
        /// every affected adjacent bone's WorldCoordinate must be updated to keep the bone chain together.
        /// </summary>
        /// <param name="index">when the bones should updated, because CalculateCCD changed their destinations</param>
        private void UpdateBones(int index)
        {
            for (int j = index; j < bones.Count - 1; j++)
            {
                bones.ElementAt(j + 1).WorldCoordinate = (bones.ElementAt(j).Effector);
            }
        }

        /// <summary>
        /// Updates all the representation parameters for every bone 
        /// including orienations and positionsin this bonechain 
        /// </summary>
        public void Update()
        {
            foreach (Bone bone in bones) bone.Update();
        }

        /// <summary>
        /// This function calculates an angle between two vectors
        /// the cross product which is orthogonal to the two vectors is the most common orientation vector 
        /// for specifing the angle's direction.
        /// </summary>
        /// <param name="v0">the first vector </param>
        /// <param name="v1">the second vector </param>
        /// <param name="crossProductOfV0andV1">the cross product of the first and second vector </param>
        /// <returns>the angle between the two vectors in radians</returns>
        private float CalculateAngle(Vector3 v0, Vector3 v1, Vector3 crossProductOfV0andV1)
        {
            Vector3 n0 = Vector3.Normalize(v0);
            Vector3 n1 = Vector3.Normalize(v1);
            Vector3 NCross = Vector3.Cross(n1, n0);
            NCross.Normalize();
            float NDot = Vector3.Dot(n0, n1);
            if (float.IsNaN(NDot)) NDot = 0;
            if (NDot > 1) NDot = 1;
            if (NDot < -1) NDot = -1;
            float a = (float)Math.Acos(NDot);
            if ((n0 + n1).Length() < 0.01f) return (float)Math.PI;
            return Vector3.Dot(NCross, crossProductOfV0andV1) >= 0 ? a : -a;
        }



    }
}
